#pragma once

#include <rclcpp/rclcpp.hpp>
#include <navigation/nav_core.hpp>
#include <navigation/cmd_publisher/twist_publisher.hpp>
#include <navigation/cmd_publisher/mani_cmd_publisher.hpp>
#include <thread>
#include <atomic>
#include <algorithm>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>

class DeviationBaseController {
public:
    explicit DeviationBaseController(
        rclcpp::Node::SharedPtr node,
        std::shared_ptr<StateManager> state_manager,
        double hz
    ): node_(node), state_manager_(state_manager), hz_(hz) {
        twist_publisher_ = std::make_unique<TwistPublisher>(node_, 10.0);
        mani_cmd_publisher_ = std::make_unique<ManiCmdPublisher>(node_);
        controller_thread_ = std::thread(&DeviationBaseController::controller_loop, this);
        RCLCPP_INFO(node_->get_logger(), "DeviationBaseController started with frequency: %.2f Hz", hz_);
        mani_cmd_publisher_->head_up_little();  // 建图需要水平mid-360
        // start();
    }

    virtual ~DeviationBaseController() {
        pause();
        running_ = false;
        if (controller_thread_.joinable()) {
            controller_thread_.join();
        }
    }

    void start() {
        pause_ = false;
        mani_cmd_publisher_->head_down_little();
        RCLCPP_INFO(node_->get_logger(), "Starting DeviationController...");
    }

    void pause() {
        if (pause_) return;
        pause_ = true;
        mani_cmd_publisher_->head_origin();
        twist_publisher_->stance();
        RCLCPP_INFO(node_->get_logger(), "DeviationController is paused.");
    }

    void controller_loop() {
        rclcpp::Rate rate(hz_);
        while (rclcpp::ok() && running_) {
            auto robot2targets = state_manager_->get_robot2targets();
            if (robot2targets.size() <= 1 || pause_ || x_lin_max == 0.0) {
                pause();
                rate.sleep();
                continue;
            }
            twist_publisher_->walk();
            std::tie(target_dist_threshold_, target_yaw_threshold_) = state_manager_->get_target_thresholds();
            auto robot_pose = robot2targets.head()->data;
            auto target_node = robot2targets.second_node();
            if (target_node == nullptr) continue;
            auto target_pose = target_node->data;
            auto deviation = target_pose - robot_pose;
            auto [x_lin, y_lin, z_ang] = process_deviation(deviation, robot_pose);
            x_lin = std::clamp(x_lin, x_lin_min.load(), x_lin_max.load());
            y_lin = std::clamp(y_lin, y_lin_min.load(), y_lin_max.load());
            z_ang = std::clamp(z_ang, z_ang_min.load(), z_ang_max.load());
            // x_lin = (x_lin + last_x_lin_) / 2.0;  // 平滑处理
            // y_lin = (y_lin + last_y_lin_) / 2.0;  // 平滑处理
            // z_ang = (z_ang + last_z_ang_) / 2.0;  // 平滑处理
            last_x_lin_ = x_lin;
            last_y_lin_ = y_lin;
            last_z_ang_ = z_ang;
            twist_publisher_->set_twist(x_lin, y_lin, z_ang);
            rate.sleep();
        }
    }

    bool is_paused_() const { return pause_; }

    virtual std::tuple<double, double, double> process_deviation(
        const PoseInfo& deviation, const PoseInfo& robot_pose
    ) = 0;

    std::atomic<double> x_lin_min{-0.3}, x_lin_max{0.3};
    std::atomic<double> y_lin_min{-0.1}, y_lin_max{0.1};
    std::atomic<double> z_ang_min{-0.3}, z_ang_max{0.3};

protected:
    rclcpp::Node::SharedPtr node_;
    std::shared_ptr<StateManager> state_manager_;
    double hz_;
    std::unique_ptr<TwistPublisher> twist_publisher_;
    std::unique_ptr<ManiCmdPublisher> mani_cmd_publisher_;

    double last_x_lin_{0.0}, last_y_lin_{0.0}, last_z_ang_{0.0};

    std::atomic<bool> running_{true}, pause_{true};
    std::thread controller_thread_;
    
    double target_dist_threshold_{0.1}, target_yaw_threshold_{0.1};
};

class EasyDeviationController : public DeviationBaseController {
    using DeviationBaseController::DeviationBaseController;  // 继承构造函数

private:
    std::tuple<double, double, double> process_deviation(
        const PoseInfo& deviation, const PoseInfo& robot_pose
    ) override {
        auto [dist_norm, yaw_norm] = deviation.norm();
        double x_lin = 0.0, y_lin = 0.0, z_ang = 0.0;
        auto q_robot = robot_pose.to_tf_quaternion();

        // 朝向目标点移动yaw误差
        auto delta = tf2::Vector3(deviation.point.x, deviation.point.y, deviation.point.z);
        delta = tf2::quatRotate(q_robot.inverse(), delta);
        double roll, pitch, yaw;
        tf2::Matrix3x3(q_robot).getRPY(roll, pitch, yaw);
        double face_yaw_error = std::atan2(delta.y(), delta.x());
        double roll_error, pitch_error, yaw_error;
        tf2::Matrix3x3(deviation.to_tf_quaternion()).getRPY(roll_error, pitch_error, yaw_error);

        if (!flag_targe_yaw_) {
            // Pase1: 优先朝向目标, 到达目标点附近后再调整朝向
            if (dist_norm >= face_it_dist_threshold_ && std::abs(face_yaw_error) >= target_yaw_threshold_) {
                z_ang = std::clamp(face_yaw_error * z_ang_alpha_, z_ang_min.load(), z_ang_max.load());
            }

            // Pase2: 已经朝向目标点, 执行平移
            if ((std::abs(face_yaw_error) < face_it_yaw_threshold_ || dist_norm < face_it_dist_threshold_)
                && dist_norm >= target_dist_threshold_
            ) {
                if (std::abs(delta.x()) > std::abs(delta.y())) {
                    x_lin = std::clamp(delta.x() * x_lin_alpha_, x_lin_min.load(), x_lin_max.load());
                } else {
                    y_lin = std::clamp(delta.y() * y_lin_alpha_, y_lin_min.load(), y_lin_max.load());
                }
            }

            if (dist_norm < target_dist_threshold_) {
                flag_targe_yaw_ = true;  // 已经接近目标点, 开始调整朝向
            }
        } else {
            // Pase3: 已接近目标点, 执行朝向调整 (当flag_targe_yaw_为true时优先调整角度, 无视平移误差)
            if (yaw_norm >= target_yaw_threshold_) {
                z_ang = std::clamp(yaw_error * z_ang_alpha_, z_ang_min.load(), z_ang_max.load());
            } else {
                flag_targe_yaw_ = false;
            }
        }

        RCLCPP_INFO(node_->get_logger(), "Deviation: dist_norm=%.2f, yaw_norm=%.2f, x_lin=%.2f, y_lin=%.2f, z_ang=%.2f, delta_x=%.2f, delta_y=%.2f, yaw_error=%.2f, face_yaw_error=%.2f",
            dist_norm, yaw_norm, x_lin, y_lin, z_ang, delta.x(), delta.y(), yaw_error, face_yaw_error);

        return {x_lin, y_lin, z_ang};
    }

    // 离目标点还差多少距离时无需继续控制朝向
    double face_it_dist_threshold_ = 0.30;  // 30 cm
    // 判断是否已经朝向目标点, 可以开始平移
    double face_it_yaw_threshold_ = M_PI / 18.0;  // 10 degrees
    double x_lin_alpha_ = 0.8;  // 平移速度系数
    double y_lin_alpha_ = 0.8;  // 平移速度系数
    double z_ang_alpha_ = 0.8;  // 旋转速度系数
    bool flag_targe_yaw_ = false;  // 是否仅调整目标角
};
